/*
* @File Name steppers.c
* @File Path M:\MAS2\PRM_Robotic_Arm\PRMCcode\coding\stepper\steppers.c
* @Author: Ruige_Lee
* @Date:   2019-04-22 17:09:51
* @Last Modified by:   Ruige_Lee
* @Last Modified time: 2019-05-19 16:06:03
* @Email: 295054118@whut.edu.cn
* @page: https://whutddk.github.io/
*//*
* @File Name steppers.c
* @File Path M:\MAS2\PRM_Robotic_Arm\PRMCcode\coding\trajectory\stepper\steppers.c
* @Author: Ruige_Lee
* @Date:   2019-04-17 14:12:43
* @Last Modified by:   Ruige_Lee
* @Last Modified time: 2019-04-22 15:20:00
* @Email: 295054118@whut.edu.cn"
*/



#include <stdint.h>
#include <string.h>




#include "steppers.h"


int16_t stepperFIFO[2][STEPPER_FIFO_DP][6];

uint16_t FIFO_read_prt[2];
uint16_t FIFO_write_prt[2];
uint8_t FIFO_full[2];
uint8_t FIFO_empty[2];

int32_t stepper_initialize() 
{
	pl_stepper_reset();
	pl_stepper_set();

	stepper_disable(0);
	stepper_PowerDown(0);

	stepper_disable(1);
	stepper_PowerDown(1);
	
	return 0;
}



int32_t stepper_enable( uint8_t robot )
{

	memset(stepperFIFO[robot], 0 ,sizeof(stepperFIFO[robot]));
	FIFO_read_prt[robot] = 0;
	FIFO_write_prt[robot] = 0;
	FIFO_full[robot] = 0;
	FIFO_empty[robot] = 1;

	for ( uint8_t i = 0; i < 2 ; i ++ )
	{
		for ( uint8_t j = 0; j < 6 ; j ++  )
		{
			pl_stepper_param_write( i, j, 0 );
		}
	}



	pl_stepper_safty_enable(robot);
	return 0;
}


int32_t stepper_disable( uint8_t robot )	
{

	memset(stepperFIFO[robot], 0 ,sizeof(stepperFIFO[robot]));
	FIFO_read_prt[robot] = 0;
	FIFO_write_prt[robot] = 0;
	FIFO_full[robot] = 0;
	FIFO_empty[robot] = 1;

	for ( uint8_t i = 0; i < 2 ; i ++ )
	{
		for ( uint8_t j = 0; j < 6 ; j ++  )
		{
			pl_stepper_param_write( i, j, 0 );
		}
	}

	pl_stepper_safty_disable(robot);

	return 0;
}

int32_t stepper_PowerUp(uint8_t robot)
{
	pl_stepper_powerOn(robot);
	return 0;
}

int32_t stepper_PowerDown(uint8_t robot)
{
	pl_stepper_powerOff(robot);
	return 0;
}

float stepper_getRad(uint8_t robot, uint8_t axis)
{
	uint32_t regValue = pl_stepper_param_read( robot, axis );
	float angle;
	switch (axis)
	{
		case(0):

			angle = ( (int32_t)((uint32_t)(regValue) - 0x80000000) ) / (float)(posDiv[0]) / ( 27. / 4. ) * 0.0314159;
			angle = angle + 1.570795;
			break;
		case(1):
			angle = ( (int32_t)((uint32_t)(regValue) - 0x80000000) ) / (float)(posDiv[1]) / ( 123. / 8. ) * 0.0314159;
			angle = angle - 1.570795;
			break;
		case(2):
			angle = ( (int32_t)((uint32_t)(regValue) - 0x80000000) ) / (float)(posDiv[2]) / ( 45. / 4. ) * 0.0314159;
			break;
		case(3):
			angle = ( (int32_t)((uint32_t)(regValue) - 0x80000000) ) / (float)(posDiv[3]) / ( 225. / 16. ) * 0.0314159;
			break;
		case(4):
			angle = ( (int32_t)((uint32_t)(regValue) - 0x80000000) ) / (float)(posDiv[4]) / ( 225. / 16. ) * 0.0314159;
			angle = angle + 1.570795;
			break;
		case(5):
			angle = ( (int32_t)((uint32_t)(regValue) - 0x80000000) ) / (float)(posDiv[5]) / 5.18 * 0.0314159;
			break;
	}
	return angle;
}


int32_t stepper_rad2Pluse(uint8_t axis, float rad)
{
	int32_t pluseCnt;
	float calTemp;
	switch (axis)
	{
		case (0):
			calTemp =  rad * posDiv[0] * ( 27. / 4. ) / 0.0314159 ;
			break;
		case (1):
			calTemp =  rad * posDiv[1] * ( 123. / 8. ) / 0.0314159 ;
			break;
		case (2):
			calTemp =  rad * posDiv[2] * ( 45. / 4. ) / 0.0314159 ;
			break;
		case (3):
			calTemp =  rad * posDiv[3] * ( 225 / 16. ) / 0.0314159 ;
			break;
		case (4):
			calTemp =  rad * posDiv[4] * ( 225 / 16. ) / 0.0314159 ;
			break;
		case (5):
			calTemp =  rad * posDiv[5] * 5.18  / 0.0314159 ;
			break;
	}

	//四舍五入
	if ( calTemp >= 0 )
	{
		pluseCnt = (int32_t)( calTemp + 0.5 );
	}
	else
	{
		pluseCnt = (int32_t)( calTemp - 0.5 );
	}


	return pluseCnt;
}



static int32_t stepper_fifoPop(uint8_t robot,int16_t* robotConfig)
{
	uint16_t FIFO_rdIndex = FIFO_read_prt[robot];
	if ( FIFO_empty[robot] == 1 )
	{
		robotConfig[0] = 0;
		robotConfig[1] = 0;
		robotConfig[2] = 0;
		robotConfig[3] = 0;
		robotConfig[4] = 0;
		robotConfig[5] = 0;
	}
	else
	{
		robotConfig[0] = stepperFIFO[robot][ FIFO_rdIndex ][0];
		robotConfig[1] = stepperFIFO[robot][ FIFO_rdIndex ][1];
		robotConfig[2] = stepperFIFO[robot][ FIFO_rdIndex ][2];
		robotConfig[3] = stepperFIFO[robot][ FIFO_rdIndex ][3];
		robotConfig[4] = stepperFIFO[robot][ FIFO_rdIndex ][4];
		robotConfig[5] = stepperFIFO[robot][ FIFO_rdIndex ][5];
	

		FIFO_read_prt[robot] ++;

		if ( FIFO_read_prt[robot] != STEPPER_FIFO_DP )
		{
			;
		}
		else
		{
			FIFO_read_prt[robot] = 0;
		}
	}

	if ( FIFO_read_prt[robot] != FIFO_write_prt[robot] )
	{
		FIFO_empty[robot] = 0;
	}
	else
	{
		FIFO_empty[robot] = 1;
	}
	return 0;
}



int32_t stepper_fifoPush(uint8_t robot,int16_t* robotConfig)
{
	uint16_t FIFO_wrIndex = FIFO_write_prt[robot];

	if ( ( FIFO_write_prt[robot] != FIFO_read_prt[robot] - 1 )
			&& ( (FIFO_write_prt[robot] != STEPPER_FIFO_DP - 1) || (FIFO_read_prt[robot] != 0) ) )
	{
		FIFO_full[robot] = 0;
	}
	else
	{
		FIFO_full[robot] = 1;
	}

	if ( FIFO_full[robot] == 0 )
	{
		stepperFIFO[robot][ FIFO_wrIndex ][0] = robotConfig[0];
		stepperFIFO[robot][ FIFO_wrIndex ][1] = robotConfig[1];
		stepperFIFO[robot][ FIFO_wrIndex ][2] = robotConfig[2];
		stepperFIFO[robot][ FIFO_wrIndex ][3] = robotConfig[3];
		stepperFIFO[robot][ FIFO_wrIndex ][4] = robotConfig[4];
		stepperFIFO[robot][ FIFO_wrIndex ][5] = robotConfig[4];

		FIFO_write_prt[robot] ++;
		

		if ( FIFO_write_prt[robot] != STEPPER_FIFO_DP )
		{
			;
		}
		else
		{
			FIFO_write_prt[robot] = 0;
		}
		return 0;

	}
	else
	{
		return -1;
	}
}


void stepper_handler(void)
{
	int16_t robotConfig[6];

	for ( uint8_t i = 0; i < 2; i++ )
	{
		stepper_fifoPop(i,robotConfig);
		
		for ( uint8_t j = 0; j < 6; j++ )
		{
			pl_stepper_param_write( i, j, robotConfig[j] );
		}
	}	
}




